#include "Arm.h"

Arm::Arm(): j4310_r1("can0", 0x51, 0x41, "arm_r1", true, false), j4310_r2("can0", 0x52, 0x42, "arm_r2", true, false),
            j4310_r3("can0", 0x53, 0x53, "arm_r3", true, false) {
    j4310_r1.control(MM::J4310Enable{});
    j4310_r2.control(MM::J4310Enable{});
    j4310_r3.control(MM::J4310Enable{});

    j4310_r1.control(MM::J4310ZeroPosition{});
    j4310_r2.control(MM::J4310ZeroPosition{});
    j4310_r3.control(MM::J4310ZeroPosition{});
}


Arm &Arm::getInstance() {
    static Arm _instance;
    return _instance;
}

void Arm::posControl(const float r1, const float r2, const float r3) {
    j4310_r1.control(MM::J4310PosVelControl{r1, 3.14});
    j4310_r2.control(MM::J4310PosVelControl{r2, 3.14});
    j4310_r3.control(MM::J4310PosVelControl{r3, 3.14});
}
